/*
Szymon Rusinkiewicz
Princeton University

rtsc.cc
Real-time suggestive contours
*/

#include "stdafx.h"
#include "mesh.hpp"
//#include "saliency.h"

//#include "vld.h" //Memory leak detector head file
#include <stdio.h>
#include <stdlib.h>
#include "GL/glui.h"
#include "TriMesh.h"
#include "TriMesh_algo.h"
#include "XForm.h"
#include "GLCamera.h"
#include "timestamp.h"
#include <algorithm>
#include "miinclude.h"
#include "windows.h"
#include "Commdlg.h"
#include <cv.h>
#include <cxcore.h>
#include <highgui.h>
#include "Wire.h"
#include "math.h"
//#include "rtscHelper.cpp"
#include "pso.h"



#include <iostream>

using std::min;
using std::max;
using std::swap;
#define  BIG   1e20
#define SMALL -1e20
#define LITTLE 1e-6
//For view segmentation
int  phai=0;
int theta=0;
//#define NEEDSAMPLING 1
#define TWOPI 6.2831852
#define LEFT -1.0
#define RIGHT 1.0
#define BOTTOM -1.0
#define TOP 1.0

#define NOEDGE 255
#define POSSIBLE_EDGE 128
#define EDGE 0
#define Thresh 3

//State of Mouse pressing 
#define STATE_NONE    0 
#define STATE_DRAWING     1
#define STATE_HITING    2 
#define eps 0.000001

//icosahedron lonely variables
#define V_N0 12		//level 0  
#define V_N1 42		//level 1  
#define V_N2 162	//level 2   
#define V_N3 642	//level 3
#define V_N4 2562	//level 4
#define V_N5 10242	//level 5
float VSC[V_N5-1][2]={0};
vector <vec2> ico5SC;

/*added by libo,20080829,begin*/
int GW=300,GH=300;	//window size

int  whichMethod, whichColor, whichBruteLevel, whichShading, bruteID=0, needVis=1;
int PSOinit, PSOdim, PSOiteration, seeRoutePSO, seeAxes;
float PSOinertia, PSOglobal, PSOlocal;
point pointNol, noOptBest, alignAxes[3];

particlesPSO flock;  int blink;//particles
float tmpArea[100000]={0};
float pixelColorBuf[800][800][3];
static float AR;
float scale_ratio=1.0;//1.5; //control the ratio of projection,default [-1.5,1.5]
float  flapcost, xUser, yUser, zUser;
GLUI *glui;
GLenum dm=GL_RENDER; //drawing mode

TriMesh *themesh, *icoMesh0, *icoMesh1, *icoMesh2, *icoMesh3, *icoMesh4, *icoMesh5;
Mesh modelMesh;
GLCamera camera;
xform xf;
int LIGHTING= TRUE, saliencyMode=FALSE;
int main_win,second_win,third_win, fourth_win;
void disp2();
void disp3();
void disp4();
void drawPSO(particlesPSO flock);
void need_redraw();
//void drawfeaturepoints(vector<Point> Points);
void mymouse(int button, int state, int x, int y);
void reshape1(int w, int h);
void setEnv(); 
void canny_edge(double **BW);
void KeyFunc(unsigned char key, int x, int y);
int  modelfile();
int  modelname();
void quicksort(float arr[],int beg,int end);
void drawModel();
void drawIcos(TriMesh *a);
float calVisibleSaliency();

// Clear the screen and reset OpenGL modes to something sane
void cls()
{
	glDisable(GL_DITHER);
	glDisable(GL_BLEND);
	glDisable(GL_DEPTH_TEST);
	glDisable(GL_NORMALIZE);
	glDisable(GL_LIGHTING);
	glDisable(GL_COLOR_MATERIAL);
	glClearColor(1,1,1,0);
	glClearDepth(1);
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
}

// Draw the scene
void disp1()
{
	timestamp t1 = now();
	// Set up camera and clear the screen
	GLUI_Master.auto_set_viewport();
	camera.setupGL(xf * themesh->bsphere.center, themesh->bsphere.r);
	cls(); // Clear the screen and reset OpenGL modes to something sane
	point v;
   
	// Draw the mesh using the correct drawing style
	glPushMatrix();
	glMultMatrixd((double *)xf);

	 glPopMatrix();
	 glutSwapBuffers();
	 fflush(stdout);	
	 if (camera.autospin(xf))
		need_redraw();	
}

// Signal a redraw
void need_redraw()
{	
	glutPostRedisplay();
}

 	
  float axis1[3]={1,0,0};  //rotate about X axis for phai
  float axis2[3]={0,1,0};  //rotate about Y axis for theta
  float axis3[3]={0,0,1};  //rotate about Z axis for theta
// Set the view to look at the middle of the mesh, from reasonably far away
int pose=0;

void resetView()
{
	xf=xform::trans(-themesh->bsphere.center);
	xf = xf*xform::trans(0, 0, -10.0f * themesh->bsphere.r);
	camera.stopspin();
}

void recoverPoseXYZ(float x, float y, float z)
{
	resetView();
	float angle=acos(z);
	xf=xf*xform::rot(angle,y,-1*x,0);
	glutSetWindow(second_win);
	disp2();
}

void readIcos()
{
	int i; vec2 sc;
	Color c = Color::black();

	//read icosahedron with depth level 0,1,2,3,and 4 (12, 42, 162, 642, and 2562 vertices)
	icoMesh0 = TriMesh::read("..\\icos\\icos0.off");
	icoMesh1 = TriMesh::read("..\\icos\\icos1.off");
	icoMesh2 = TriMesh::read("..\\icos\\icos2.off");
	icoMesh3 = TriMesh::read("..\\icos\\icos3.off");
	icoMesh4 = TriMesh::read("..\\icos\\icos4.off");
	icoMesh5 = TriMesh::read("..\\icos\\icos5.off");
	//create neighbor data structure and others
	icoMesh0->need_neighbors();
	icoMesh0->need_faces();
	icoMesh0->need_bsphere();
	icoMesh0->need_normals();
	for (i=0;i<(int)icoMesh0->vertices.size();i++)
		icoMesh0->colors.push_back(c);

	icoMesh1->need_neighbors();
	icoMesh1->need_faces();
	icoMesh1->need_bsphere();
	icoMesh1->need_normals();
	for (i=0;i<(int)icoMesh1->vertices.size();i++)
		icoMesh1->colors.push_back(c);

	icoMesh2->need_neighbors();
	icoMesh2->need_faces();
	icoMesh2->need_bsphere();
	icoMesh2->need_normals();
	for (i=0;i<(int)icoMesh2->vertices.size();i++)
		icoMesh2->colors.push_back(c);

	icoMesh3->need_neighbors();
	icoMesh3->need_faces();
	icoMesh3->need_bsphere();
	icoMesh3->need_normals();
	for (i=0;i<(int)icoMesh3->vertices.size();i++)
		icoMesh3->colors.push_back(c);

	icoMesh4->need_neighbors();
	icoMesh4->need_faces();
	icoMesh4->need_bsphere();
	icoMesh4->need_normals();
	for (i=0;i<(int)icoMesh4->vertices.size();i++)
		icoMesh4->colors.push_back(c);

	icoMesh5->need_neighbors();
	icoMesh5->need_faces();
	icoMesh5->need_bsphere();
	icoMesh5->need_normals();
	for (i=0;i<(int)icoMesh5->vertices.size();i++)
		icoMesh5->colors.push_back(c);
	//create spherical coordinate data structure (since we prefer to avoid modifying
	//other people's header, it will be put in VP arrays). Icosahedron vertices are arranged subsequently based on its level
	//or in other words, vertex 1 to 12 will be in the same order as for icosahedron level 0 and 1, vertex 1 to 42 for level 1 and 2, etc.
	//Thus we can simply store the spherical coordinate in one single array
	float x, y, z, phai, theta;
	for(int i=0;i<V_N5;i++)
	{
		XYZToSphere(sc,icoMesh5->vertices[i]);
		ico5SC.push_back(sc);
		/*x = icoMesh5->vertices[i][0]; y = icoMesh5->vertices[i][1]; z = icoMesh5->vertices[i][2];
		phai=acos(y/sqrt(x*x+y*y+z*z));
		theta=atan2(z, x);
		if (theta < 0)	theta = TWOPI - fabs(theta);
		VSC[i][0]=phai;
		VSC[i][1]=theta;*/
	}
}

void InsertIndex(char *des,int index)
{
    char T[4];
    int q,b,s,g;
	
	q=index/1000;
	b=(index-1000*q)/100;
	s=(index-1000*q-100*b)/10;
	g=index-1000*q-100*b-10*s;
	if(index<10)
	{
	     T[0]=char(int('0'+g));
	     strncat(des,T,1);
	}
	else if (index<=99)
	{ 
		T[0]=char(int('0'+s));T[1]=char(int('0'+g));
		strncat(des,T,2);
	}
	else if (index<=999)
	{
		T[0]=char(int('0'+b));T[1]=char(int('0'+s));T[2]=char(int('0'+g));
		strncat(des,T,3);
	}
	else
	{
		T[0]=char(int('0'+q));T[1]=char(int('0'+b));T[2]=char(int('0'+s));T[3]=char(int('0'+g));
		strncat(des,T,4);
	}
}
void quicksort(float arr[],int beg,int end)
{
	if (end  >= beg + 1) 
  	{
  		float piv = arr[beg];
		int k = beg + 1, r = end;
    
		while (k < r) 
    		{
      			if (arr[k] < piv) 
        			k++;
      			else 
        			swap(arr[k], arr[r--]);
    		}
		if (arr[k] < piv){
		
			swap(arr[k],arr[beg]);
			
			quicksort(arr, beg, k);
			quicksort(arr, r, end);			
		}else {
			if (end - beg == 1)
  				return;
  				
			swap(arr[--k],arr[beg]);
			quicksort(arr, beg, k);
			quicksort(arr, r,   end);			
		}
  	}
}


// Handle mouse button and motion events
static unsigned buttonstate = 0;

void mousemotionfunc(int x, int y)
{
	static const Mouse::button physical_to_logical_map[] = {
		Mouse::NONE, Mouse::ROTATE, Mouse::MOVEXY, Mouse::MOVEZ,
		Mouse::MOVEZ, Mouse::MOVEXY, Mouse::MOVEXY, Mouse::MOVEXY,
	};
	Mouse::button b = Mouse::NONE;
	if (buttonstate & (1 << 3))
		b = Mouse::WHEELUP;
	else if (buttonstate & (1 << 4))
		b = Mouse::WHEELDOWN;
	else
		b = physical_to_logical_map[buttonstate & 7];
	//reset here, added by libo
	if (b == Mouse::ROTATE)
	{	
	}
     
	camera.mouse(x, y, b,
		     xf * themesh->bsphere.center, themesh->bsphere.r,
		     xf);

      	 /*
	xf.rot(90/180,0,0,1);
	vec axis(0,0,1);
	rot(themesh,30/180,axis);
       */
	need_redraw();
}

void mousebuttonfunc(int button, int state, int x, int y)
{
	if (state == GLUT_DOWN)
		buttonstate |= (1 << button);
	else
		buttonstate &= ~(1 << button);

	mousemotionfunc(x, y);	
}


// Keyboard callback
void keyboardfunc(unsigned char key, int x, int y)
{
	switch (key) {
		case 'Q':
		case 'q':
			exit(0);		
	}
	need_redraw(); 
	GLUI_Master.sync_live_all();
}

void skeyboardfunc(int key, int x, int y)
{
	need_redraw(); 
	GLUI_Master.sync_live_all();
}

void reshape(int x, int y)
{
	GLUI_Master.auto_set_viewport();
	cls();
	glutSwapBuffers();
	need_redraw();
}


void usage(const char *myname)
{
	fprintf(stderr, "Usage: %s [-options] infile\n", myname);
	exit(1);
}
float calSymmetry()
{
	int i, j, k;
	float area=0,m=0;

	//addition
	float centerX=0,centerY=0, areaY=0, areaX=0;
	int x,y;

	int View[4];
	glutSetWindow(second_win);
	glGetIntegerv(GL_VIEWPORT, View);
	int width = View[2], height = View[3];
	centerX=width/2;
	centerY=height/2;

	for (i = 0; i < width; i++)
	for (j = 0; j < height; j++)
	  for (k = 0; k < 3; k++)
		  pixelColorBuf[i][j][k] = 0;
	glReadBuffer(GL_FRONT);
	glReadPixels(View[0], View[1], width, height, GL_RGB, GL_FLOAT, pixelColorBuf);
	//maximize symmetry along y axis
	//check left to right
	for(i=0;i<width/2;i++)
	{
		for (j=0;j<height;j++)
		{
			if (pixelColorBuf[i][j][2]==0.0)
			{
				x=width-i;
				if(x!=i)
				if(pixelColorBuf[x][j][2]==0.0)
					areaY+=log(centerX-i);
			}
		}
	}
	//maximize symmetry along x axis
	//check up to bottom
	for(i=0;i<width;i++)
	{
		for (j=0;j<height/2;j++)
		{
			if (pixelColorBuf[i][j][2]==0.0)
			{
				 y=height-j;
				 if(y!=j)
					 if(pixelColorBuf[x][j][2]==0.0)
						areaX+=log(centerY-j);
			}
		}
	}

	area = areaX;//areaY; //make the object view as tall as possible but as slim as possible by playing around with symmetry property
	return area;
}

float calArea(int v)
{
	int i, j, k;
	float area=0,m=0;

	int View[4];
	glutSetWindow(second_win);
	glGetIntegerv(GL_VIEWPORT, View);
	int width = View[2], height = View[3];

	for (i = 0; i < width; i++)
		for (j = 0; j < height; j++)
			for (k = 0; k < 3; k++)
				pixelColorBuf[i][j][k] = 0;
	glReadBuffer(GL_FRONT);
	glReadPixels(View[0], View[1], width, height, GL_RGB, GL_FLOAT, pixelColorBuf);
	for (i = 0; i < width; i++){
		for (j = 0; j < height; j++)  
			if (pixelColorBuf[i][j][2] == 0.0) 
				area=area+1;
	}
  area=area/(GW*GH);
  if(bruteID==1)
	tmpArea[v]=area;
  return area;
}

float ComputeCurrentPos(int pos, int icoLv, int method)
{
	float phai0,theta0,x,y,z,s;
	/*
	x=VS2[pos][0];
	y=VS2[pos][1];
	z=VS2[pos][2];
	*/
	switch(icoLv)
	{
	case 0:
		x=icoMesh0->vertices[pos][0]; y=icoMesh0->vertices[pos][1]; z=icoMesh0->vertices[pos][2]; break;
	case 1:
		x=icoMesh1->vertices[pos][0]; y=icoMesh1->vertices[pos][1]; z=icoMesh1->vertices[pos][2]; break;
	case 2:
		x=icoMesh2->vertices[pos][0]; y=icoMesh2->vertices[pos][1]; z=icoMesh2->vertices[pos][2]; break;
	case 3:
		x=icoMesh3->vertices[pos][0]; y=icoMesh3->vertices[pos][1]; z=icoMesh3->vertices[pos][2]; break;
	case 4:
		x=icoMesh4->vertices[pos][0]; y=icoMesh4->vertices[pos][1]; z=icoMesh4->vertices[pos][2]; break;
	case 5:
		x=icoMesh4->vertices[pos][0]; y=icoMesh4->vertices[pos][1]; z=icoMesh4->vertices[pos][2]; break;
	}
	recoverPoseXYZ(x,y,z);
	switch(method)
	{
	case 1: s=calArea(pos); break;
	case 2: s=calSymmetry(); break;
	}
	return s;	
}

/*
void findNeighbors(int pos,int level,vector <int> &views)
{
   int i,j;	
   point V;
   float dist[V_N3]={0},sortdist[V_N3]={0}; //temporary array var to store 'distance' and 'sorted distance'
   switch(level)
   {
      case 1:   

		  //dist=new float[V_N1];
		  //sortdist=new float[V_N1];
		  
		  for(i=0;i<V_N1;i++)
		  {
			  dist[i]=pow(VS1[i][0]-VS1[pos][0],2)+pow(VS1[i][1]-VS1[pos][1],2)+pow(VS1[i][2]-VS1[pos][2],2);
			  sortdist[i]=dist[i];
		  }
		  quicksort(sortdist,0,V_N1-1);	
		  for(j=0;j<V_N1;j++)
		  {
			  if(dist[j]<=sortdist[5]+1e-5)
			  {
				  views.push_back(j);
			  }
		  }	
		  break;
      case 2:   	
		  //dist=new float[V_N2];
		  //sortdist=new float[V_N2];
		  for(i=0;i<V_N2;i++)
		  {
			  dist[i]=pow(VS2[i][0]-VS2[pos][0],2)+pow(VS2[i][1]-VS2[pos][1],2)+pow(VS2[i][2]-VS2[pos][2],2);
			  sortdist[i]=dist[i];		 
		  }
		  quicksort(sortdist,0,V_N2-1);
		  for(j=1;j<V_N2;j++)
		  {
			  if(dist[j]<=sortdist[5]+1e-5)
			  {
				  views.push_back(j);
			  }
		  }		 
		  break;
      case 3:   	
		 // dist=new float[V_N3];
		  //sortdist=new float[V_N3];		  
		  for(i=0;i<V_N3;i++)
		  {
			  dist[i]=pow(VS3[i][0]-VS3[pos][0],2)+pow(VS3[i][1]-VS3[pos][1],2)+pow(VS3[i][2]-VS3[pos][2],2);
			  sortdist[i]=dist[i];		 
		  }
		  quicksort(sortdist,0,V_N3-1);	
		  for(j=0;j<V_N3;j++)
		  {
			  if(dist[j]<=sortdist[5]+1e-5)
			  {
				  views.push_back(j);
			  }
		  }	
		  break;		  
   }   	
}
*/


void neighbourPos(int pos, int lv, vector <int> &views)
{
	vector<int>tmpView;
	switch (lv)
	{
		case 0:
			views=icoMesh0->neighbors[pos];
			break;
		case 1:
			views = icoMesh1->neighbors[pos];
			/*for (i=0;i<icoMesh1->neighbors[pos].size();i++)
				tmpView=icoMesh1->neighbors[views[i]];
				for(j=0;j<tmpView.size();j++)
					for (k=0;k<views.size();k++)
						if(tmpView[j]!=views[k])
							views.push_back(tmpView[j]);*/
				break;
		case 2:
			views = icoMesh2->neighbors[pos];
			break;
		case 3:
			views = icoMesh3->neighbors[pos];
			break;
		case 4:
			views = icoMesh4->neighbors[pos];
			break;
		case 5:
			views = icoMesh5->neighbors[pos];
			break;
	}
}
	
int ComputeMinMaxNeighbourViews(vector <int> &views, float &min, float &max, int lv, int method)
{//return index of minimum view
	int i, pos, j;
	min=BIG, max=SMALL; float s;
	float phai0,theta0;
	point p;
	for(i=0;i<(int)views.size();i++)
	{
		switch (lv)
		{
		case 0:
			p = icoMesh0->vertices[i];
			recoverPoseXYZ(p[0],p[1],p[2]);
			break;
		case 1:
			p = icoMesh1->vertices[i];
			recoverPoseXYZ(p[0],p[1],p[2]);
			break;
		case 2:
			p = icoMesh2->vertices[i];
			recoverPoseXYZ(p[0],p[1],p[2]);
			break;
		case 3:
			p = icoMesh3->vertices[i];
			recoverPoseXYZ(p[0],p[1],p[2]);
			break;
		case 4:
			p = icoMesh4->vertices[i];
			recoverPoseXYZ(p[0],p[1],p[2]);
			break;
		case 5:
			p = icoMesh5->vertices[i];
			recoverPoseXYZ(p[0],p[1],p[2]);
			break;		
		}
		//glutSetWindow(second_win);
		switch(method)
		{
			case 1: s=calArea(i); break;
			case 2: s=calSymmetry(); break;
		}
		if (min>s)
		{
		   min=s;
		   pos=views[i];
		 }
		if (max<s)
			max=s;
	}
	//min=minS;
	//max=maxS;
	return pos;
}

void computeColorDist(float min, float max, TriMesh *mesh, int VNUM)
{
	//gray and rgb variables
	float range = max-min, tmp, tmpRGB, maxR=1, maxG=2, maxB =3;
	int i;
	//hsv variables
	float hue, tmpHue, v=1, s=1, X, r, g, b;
	int index;
	
	for(i=0;i<VNUM;i++)
	{
		tmp=(tmpArea[i]-min)/range;
		tmpRGB=tmp*3;
		for(int k=0;k<3;k++) mesh->colors[i][k]=0;
		//map areas to colors
		if(whichColor==0)//grayscale colors
			for(int j=0;j<3;j++) mesh->colors[i][j]=tmp;
		else if(whichColor==1)
		{//rgb colors
			if(tmpRGB<maxR)mesh->colors[i][0]=tmpRGB;
			else if(tmpRGB>=maxR && tmpRGB<maxG)mesh->colors[i][1]=tmpRGB-1;
			else mesh->colors[i][2]=tmpRGB-2;
		}
		else
		{//map to HSV for natural color then remap back to rgb
			hue=(tmpArea[i]-min)/range*360; //map to 0-360
			tmpHue=(hue*330/360)-60; //map to (-60 to 270)
			if(tmpHue<0) tmpHue+=60;
			//remap into rgb space
			tmpHue/=60;			// sector 0 to 5
			index = floor(tmpHue);
			X=v*s*(1-fabs(fmod(tmpHue,2)-1));
			switch( index ) {
				case 0:
					r = v*s; g = X; b = 0;break;
				case 1:
					r = X;g = v*s;b = 0;break;
				case 2:
					r = 0;g = v*s;b = X;break;
				case 3:
					r = 0;g = X;b = v*s;break;
				case 4:
					r = X;g = 0;b = v*s;break;
				default:		//case 5:
					r = v*s;g = 0;b = X;break;
			}
			mesh->colors[i][0]=r;
			mesh->colors[i][1]=g;
			mesh->colors[i][2]=b;
		}
	}
}

void viewPointAreaBrute()
{
	bruteID=1; seeAxes=0;
	int i, pos=0, level, VNUM;
	point p;
	float minS=BIG,maxS=0,s,max,A0=0,A1=BIG,A2=0, deltatheta=0;
	switch (whichBruteLevel)
	{
	case 0: VNUM=V_N0;break;
	case 1: VNUM=V_N1;break;
	case 2: VNUM=V_N2;break;
	case 3: VNUM=V_N3;break;
	case 4: VNUM=V_N4;break;
	case 5: VNUM=V_N5;break;
	}
	for(i=0;i<VNUM;i++) tmpArea[i]=0;
	vector <point> ShortListView;
	point V;
	vector <int> views;
	timestamp t1 = now(); //initiate time counter
	LIGHTING=FALSE; saliencyMode=FALSE;
	views.clear();
	
	//stage: brute level icosahedron
	for(i=0;i<VNUM;i++)
		views.push_back(i);	
	pos=ComputeMinMaxNeighbourViews(views,s,max,whichBruteLevel,1);
	A1=s;
	switch (whichBruteLevel)
	{
	case 0: computeColorDist(s, max, icoMesh0, VNUM);noOptBest=icoMesh0->vertices[pos];break;
	case 1: computeColorDist(s, max, icoMesh1, VNUM);noOptBest=icoMesh1->vertices[pos];break;
	case 2: computeColorDist(s, max, icoMesh2, VNUM);noOptBest=icoMesh2->vertices[pos];break;
	case 3: computeColorDist(s, max, icoMesh3, VNUM);noOptBest=icoMesh3->vertices[pos];break;
	case 4: computeColorDist(s, max, icoMesh4, VNUM);noOptBest=icoMesh4->vertices[pos];break;
	case 5: computeColorDist(s, max, icoMesh5, VNUM);noOptBest=icoMesh5->vertices[pos];break;
	}
	bruteID=0;
	p=icoMesh5->vertices[pos]; alignAxes[0]=p;
	cout<<endl<<"~~~~~~~~~~~Performance and results~~~~~~~~~~~"<<endl;
	cout<<"Minimum brute at:"<<p<<" with value:"<<A1<< endl;
	printf("Elapsed time: %.2f sec.",  (now() - t1), "\n");
	LIGHTING=TRUE;
//show minimum view after 1st alignment
	recoverPoseXYZ(p[0],p[1],p[2]); //delete this part later
	glutSetWindow(second_win);//yuck, but it's neccessary
	disp2();
	cout<<endl<<"showing view obtained after 1st step ....";
	//Sleep(3000);
	LIGHTING=FALSE;

//Step2: alignment
	float angle=acos(p[2]);
	timestamp t2=now();
	float phai1=0,theta1=0,theta2=0;              //deltatheta=PI/36.0; 	//deltatheta=PI/12.0;  //radius  PI/4.0;  
	const float DeltaTheta[5]={PI/3.0,PI/6.0,PI/12.0,PI/36.0,PI/180.0};  //60,30,15,5,1
	float Lower=0, Upper=TWOPI;
	for(level=0;level<4;level++)
	{
		deltatheta=DeltaTheta[level];
		for(theta1=Lower;theta1<Upper;theta1=theta1+deltatheta)
		{
			resetView();			
			xf=xf*xform::rot(theta1, 0, 1, 0)*xform::rot(PI/2,1,0,0)*xform::rot(angle,p[1],-1*p[0],0);		
			glutSetWindow(second_win);
			disp2();
			s=calArea(i);
			if (minS>s)
			{
				minS=s;
				theta2=theta1;
			}
		}
		Lower=theta2-deltatheta;
		Upper=theta2+deltatheta;
	}	
	//for(theta1=0;theta1<Upper;theta1+=(TWOPI/360))
	//	{
	//		resetView();			
	//		xf=xf*xform::rot(theta1, axis2[0], axis2[1], axis2[2])*xform::rot(PI/2,1,0,0)*xform::rot(angle,p[1],-1*p[0],0);
	//		glutSetWindow(second_win);
	//		disp2();
	//		s=calArea(i); 			//printf("Pose:%d, View Entropy:%f\n",i,s);
	//		if (minS>s)
	//		{
	//			minS=s;
	//			theta2=theta1;
	//		}
	//}
	A2=minS; alignAxes[1]=xform::rot(theta2,0,1,0)*xform::rot(PI/2,1,0,0)*alignAxes[0];
	alignAxes[2]=alignAxes[0]%alignAxes[1]; //cross product
	LIGHTING=TRUE;
	cout<<endl<<"Minimum area for 2nd step at theta: "<<theta2<<"with value: "<<A2<<endl;
	printf("Elapsed time: %.2f sec.",  (now() - t2));
//show minimum view after 2nd step (only 2nd step)
	resetView();
	xf=xf*xform::rot(theta2, axis2[0], axis2[1], axis2[2])*xform::rot(PI/2,1,0,0)*xform::rot(angle,p[1],-1*p[0],0);
	glutSetWindow(second_win);
	disp2();
	cout<<endl<<"showing minimum view obtained during 2nd step ....";
	//Sleep(4000);
//Show pose after 1st and 2nd step
	resetView();
	xf=xf*xform::rot(-1*theta2, axis3[0], axis3[1], axis3[2])*xform::rot(angle,p[1],-1*p[0],0);
	glutSetWindow(second_win);
	disp2();
	cout<<endl<<"showing view obtained after 1st and 2nd step ....";
	//Sleep(4000);

//Step3: saliency measure of 6 cubical pose
	float visSal[6], angle1;
	saliencyMode=TRUE;
	glutSetWindow(second_win);//yuck, but it's neccessary
			disp2();
	
	//six standard cubical pose
		point fr(0,0,1), ba(0,0,-1);
		point to(0,1,0), bo(0,-1,0);
		point le(1,0,0), ri(-1,0,0), dummy;
		
		vector<point>stdPoseVP;
		stdPoseVP.push_back(fr);stdPoseVP.push_back(ba);
		stdPoseVP.push_back(to);stdPoseVP.push_back(bo);
		stdPoseVP.push_back(le);stdPoseVP.push_back(ri);
		string ps[]={"Front: ","Back: ","Top: ","Bottom: ","Left: ","Right: "};
		cout<<endl<<"oo---Saliency value---oo";
		for(i=0;i<6;i++){
			dummy=stdPoseVP[i];
			angle1=acos(dummy[2]);
			resetView();
			if(i==1)//back view apparently always fail....
				xf=xf=xf*xform::rot(PI, 0, 1,0)*xform::rot(-1*theta2, axis3[0], axis3[1], axis3[2])*xform::rot(angle,p[1],-1*p[0],0);
			else
			xf=xf*xform::rot(angle1, dummy[1], -1*dummy[0],0)*xform::rot(-1*theta2, axis3[0], axis3[1], axis3[2])*xform::rot(angle,p[1],-1*p[0],0);
			glutSetWindow(second_win);//yuck, but it's neccessary
			disp2();
			visSal[i]=calVisibleSaliency();
			cout<<endl<<ps[i]<<visSal[i];
			//Sleep(4000);
		}
	saliencyMode=FALSE;
//Show pose after 1st and 2nd step
	resetView();
	xf=xf*xform::rot(-1*theta2, axis3[0], axis3[1], axis3[2])*xform::rot(angle,p[1],-1*p[0],0);
	glutSetWindow(second_win);
	disp2();
	cout<<endl<<"showing view obtained after 1st and 2nd step ....";
	//Sleep(4000);
	seeAxes=1;
}

void visualize()
{
	viewPointAreaBrute();
	glutSetWindow(third_win);
	needVis=1;
}

void viewPointPSO(){
	//1st step of 1st alignment: initialization of the particles based on icosahedron level 1 (discrete/ grid search)
	vector <int> pos; vector<float>tmps; point p, gBestPos; particle x; 
	flock.birds.clear();
	int i, j, VNUM=V_N1, numNear=PSOinit/3+1;
	float gBest, tmp[42], minTmp=BIG, maxTmp = SMALL, A1, A2, tmpDist;
	float stepSz[2]={0.5, 3};
	float cons[3]={PSOinertia,PSOlocal, PSOglobal};
	LIGHTING=FALSE; saliencyMode=FALSE;
//	timestamp t1 = now(); //initiate time counter
	resetView();
	glutSetWindow(second_win); disp2();

	vector <int> oldShuffle, shuffle;
	//create and randomize index from 0 to number of particles
	for(i=0;i<VNUM;i++)
		oldShuffle.push_back(i);
	for(i=0;i<VNUM;i++){
		int r=i+(rand()%(VNUM-i)); // Random remaining position.
            int temp = oldShuffle[i]; oldShuffle[i] = oldShuffle[r]; oldShuffle[r] = temp;
        }
	for(i=0;i<PSOinit;i++)
		shuffle.push_back(oldShuffle[i]);
	//initialize gbest from L1 icos (purpose: to reduce overall useless iterations) and each particle's position
	gBest=BIG;
	for(i=0;i<VNUM;i++) {
		p=icoMesh1->vertices[i];
		recoverPoseXYZ(p[0],p[1],p[2]);
		tmp[i]=calArea(0);
	}
	for(i=0;i<PSOinit;i++){
		if(tmp[shuffle[i]]<gBest){
			gBest=tmp[shuffle[i]];
			gBestPos=icoMesh1->vertices[shuffle[i]];
		}
	}
	//find standard deviation of area distribution from a level 1 ico view
	//just to check if the area distribution over the sphere is good or bad
	float meanTmp=0, stdTmp=0;
	for(i=0;i<VNUM;i++)
		meanTmp+=(tmp[i]/VNUM);
	for(i=0;i<VNUM;i++)
		stdTmp+=pow((tmp[i]-meanTmp),2);
	stdTmp=sqrt(stdTmp/VNUM);

	timestamp t1 = now(); //initiate time counter
	//cout<<"standard deviation over an L1 ico is: "<<stdTmp<<endl;
//---------------------------------
	//initialize birds flock 
	for(i=0;i<PSOinit;i++){
		x.P0=icoMesh1->vertices[shuffle[i]];
		XYZToSphere(x.P0SC, x.P0);
		x.score=tmp[shuffle[i]];
		flock.birds.push_back(x);	
	}
	for(i=0;i<PSOinit;i++){// make sure to record particles position and feature for visualization etc
		flock.birds[i].P0s.push_back(flock.birds[i].P0);
		flock.birds[i].scores.push_back(tmp[shuffle[i]]);
	}
	//update initial localbest
	flock.updateNeighborPSO(numNear);
	flock.updateLBestPSO();
	//update initial global best
	flock.birds[0].gBest=gBest;
	flock.birds[0].gBestP0=gBestPos;
	XYZToSphere(flock.birds[0].gBestP0SC,gBestPos);
//-------------------------------
	//start pso iterations
	for(i=0;i<PSOiteration;i++){
		//update velocity and position
		flock.updatePosPSOsc(stdTmp/5,stepSz,cons,i,PSOiteration);
		//update score at next pos
		for(j=0;j<PSOinit;j++){
			p=flock.birds[j].P0;
			recoverPoseXYZ(p[0],p[1],p[2]);
			A1=calArea(0);
			flock.birds[j].score=A1;flock.birds[j].scores.push_back(A1);
		}
		//update local and global best
		flock.updateNeighborPSO(numNear);
		flock.updateLBestPSO();
//		flock.updtNeighborPSO(4,;
		flock.updateGBestPSO();
	}
	p=flock.birds[0].gBestP0;
	//recoverPoseXYZ(p[0],p[1],p[2]); //delete this part latter
	cout<<endl<<"~~~~~~~~~~~Performance and results~~~~~~~~~~~"<<endl;
	cout<<"Minimum PSO at:"<<flock.birds[0].gBestP0<<" with value:"<<flock.birds[0].gBest<< endl;
	printf("Elapsed time: %.2f sec.",  (now() - t1), "\n");
	//LIGHTING=TRUE;
//2nd step	
	float angle=acos(p[2]),s,minS=BIG;
	//xf=xf*xform::rot(angle,p[1],-1*p[0],0);s, minS;
	float phai1=0,theta1=0,theta2=0,deltatheta=0.0;  
	const float DeltaTheta[5]={PI/3.0,PI/6.0,PI/12.0,PI/36.0,PI/180.0};  //60,30,15,5,1
	float Lower=0, Upper=TWOPI;
	int level;
	timestamp t2 = now();
	for(level=0;level<4;level++)
	{
		deltatheta=DeltaTheta[level];
		for(theta1=Lower;theta1<Upper;theta1=theta1+deltatheta)
		{
			resetView();			
			xf=xf*xform::rot(theta1, axis2[0], axis2[1], axis2[2])*xform::rot(angle,p[1],-1*p[0],0);		
			glutSetWindow(second_win);
				
			disp2();
			s=calArea(0); 			//printf("Pose:%d, View Entropy:%f\n",i,s);
			if (minS>s)
			{
				minS=s;
				theta2=theta1;
			}
		}
		Lower=theta2-deltatheta;
		Upper=theta2+deltatheta;
	}	
	A2=minS;
	LIGHTING=TRUE;
	//Show the final pose
	resetView();	
	xf=xf*xform::rot(theta2, axis2[0], axis2[1], axis2[2])*xform::rot(angle,p[1],-1*p[0],0);		
	glutSetWindow(second_win);
	disp2();
	s=calArea(0);
	cout<<endl<<"Minimum area for 2nd step at theta: "<<theta2<<"with value: "<<A2<<endl;
	printf("Elapsed time: %.2f sec.",  (now() - t2));
}

void viewPointMultiLv(){
	int j,pos=0,level=1;
	float minS=BIG,maxS=0,s, s0,max, A0=0,A1=BIG,A2=0;

	vector <point> ShortListView;
	point V;
	vector <int> views;
	timestamp t1 = now(); //initiate time counter
	LIGHTING=FALSE;

	//stage: Level 0 icosahedron to determine initial pos to search
	for(int x= 0;x<12;x++)
		views.push_back(x);
	pos = ComputeMinMaxNeighbourViews(views,s0,max,level,1);

	//stage: subsequent level icosahedron for further refinement (level 1-5)
	for(level=1;level<5;level++)
	{
		while(1)
		{
			s0=ComputeCurrentPos(pos, level, 1);
			views.clear();
			neighbourPos(pos,level,views);
			//FindNeighbors(pos,level,views);
			j=ComputeMinMaxNeighbourViews(views,s,max,level,1);//store index of minimum neighbor
			if (s<s0)   //compare neighbor and current
			{ 
				pos=j; 
			}
			else 
			{
				A1=s0; 
				break;
			}
		}
	}
	point p;
	noOptBest=icoMesh5->vertices[pos];
	p=noOptBest;
	float angle=acos(p[2]);
	//recoverPoseXYZ(noOptBest[0],noOptBest[1],noOptBest[2]);

	float phai0,theta0;	
	phai0=VSC[pos][0];
	theta0=VSC[pos][1];
	
	//Step: 2D alignment
	float phai1=0,theta1=0,theta2=0,deltatheta=0.0;              //deltatheta=PI/36.0; 	//deltatheta=PI/12.0;  //radius  PI/4.0;  
	const float DeltaTheta[5]={PI/3.0,PI/6.0,PI/12.0,PI/36.0,PI/180.0};  //60,30,15,5,1
	float Lower=0, Upper=TWOPI;
	for(level=0;level<4;level++)
	{
		deltatheta=DeltaTheta[level];
		for(theta1=Lower;theta1<Upper;theta1=theta1+deltatheta)
		{
			resetView();			
			//xf=xf*xform::rot(theta1, axis2[0], axis2[1], axis2[2])*xform::rot(PI/2.0, axis1[0], axis1[1], axis1[2])*xform::rot(theta0, axis2[0], axis2[1], axis2[2])*xform::rot(phai0, axis1[0], axis1[1], axis1[2]);
			xf=xf*xform::rot(theta1, axis2[0], axis2[1], axis2[2])*xform::rot(PI/2.0, axis1[0], axis1[1], axis1[2])*xform::rot(angle,p[1],-1*p[0],0);
			//xf=xf*xform::rot(angle,p[1],-1*p[0],0)*xform::rot(theta1, axis2[0], axis2[1], axis2[2]);
			//xf=xf*xform::rot(theta1, axis2[0], axis2[1], axis2[2])*xform::rot(angle,p[1],-1*p[0],0);

			glutSetWindow(second_win);
			disp2();
			s=calArea(0); 			//printf("Pose:%d, View Entropy:%f\n",i,s);
			if (minS>s)
			{
				minS=s;
				theta2=theta1;
			}
		}
		Lower=theta2-deltatheta;
		Upper=theta2+deltatheta;
	}	
	A2=minS;
	LIGHTING=TRUE;
	//Show the final pose
	resetView();	
	//xf=xf*xform::rot(-PI/2.0, axis1[0], axis1[1], axis1[2])*xform::rot(theta2, axis2[0], axis2[1], axis2[2])*xform::rot(PI/2.0, axis1[0], axis1[1], axis1[2])*xform::rot(theta0, axis2[0], axis2[1], axis2[2])*xform::rot(phai0, axis1[0], axis1[1], axis1[2]);
	xf=xf*xform::rot(theta1, axis2[0], axis2[1], axis2[2])*xform::rot(PI/2.0, axis1[0], axis1[1], axis1[2])*xform::rot(angle,p[1],-1*p[0],0);
	//xf=xf*xform::rot(angle,p[1],-1*p[0],0)*xform::rot(theta1, axis2[0], axis2[1], axis2[2]);
	//xf=xf*xform::rot(theta1, axis2[0], axis2[1], axis2[2])*xform::rot(angle,p[1],-1*p[0],0);

	glutSetWindow(second_win);
	disp2();
	s=calArea(0);
	printf("\n************Result & Performance******************\n");
	printf("\rOptimal pose: %d\nView: (%f,%f,%f)\nDeltatheta=%f\nEntropy: A1=%f, A2=%f\n",pos,V[0],V[1],V[2],deltatheta,A1,A2);
	printf("Elapsed time: %.2f sec.",  (now() - t1));

}

void viewPoint()
{
	switch (whichMethod)
	{
	case 0:	viewPointMultiLv(); break;
	case 1: viewPointPSO();break;
	}
}

void CurrentViewpoint()
{
	int i=0;
  LIGHTING=FALSE;
  //disp1();
  glutSetWindow(second_win);
  disp2();
  flapcost=calArea(i);
  glui->sync_live();  
  LIGHTING=TRUE;
}
void rotateToHere(){
	float mag=sqrt(pow(xUser,2)+pow(yUser,2)+pow(zUser,2));
	if (mag==0)xUser=LITTLE;
	glui->sync_live();
	resetView();
	LIGHTING=FALSE;
	recoverPoseXYZ(xUser, yUser, zUser);
	glutSetWindow(second_win);
	flapcost=calArea(0);
	glui->sync_live();
	LIGHTING=TRUE;
	disp2();
}


int main(int argc, char *argv[])
{  
	//initialize the GLUT
	glutInitWindowSize(GW+210,GH+400);            
	glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH);
	glutInit(&argc, argv);

	if (argc < 2)
		usage(argv[0]);

	// Skip over any parameter beginning with a '-'
	int i = 1;
	while (i < argc-1 && argv[i][0] == '-')
		i++;
	const char *filename = argv[i];
	//memset(meshname,0,128);
	//strcpy(meshname,(const)filename);
	//currentmeshname=filename;
	themesh = TriMesh::read(filename);
	//Readpricinpleaxis();	
	readIcos();
	if (!themesh)
		usage(argv[0]);
 
	themesh->need_tstrips();
	themesh->need_bsphere();
	themesh->need_bbox();
	themesh->need_normals();
	themesh->need_curvatures();
	themesh->need_dcurv();
	cout<<atan2(0,0.5)<<" "<<atan2(1,0.0)<<endl;
	cout<<"Strip Size: "<<themesh->tstrips.size()<<endl;
	cout<<"Vertices Size: "<<themesh->vertices.size()<<endl;
      
   	char windowname[255];
	sprintf(windowname, "RTSC - %s", filename);
	glutInitWindowPosition(200,100);
	main_win = glutCreateWindow(windowname);
	
	printf("main_win=%d\n",main_win);

	glutDisplayFunc(disp1);
	GLUI_Master.set_glutMouseFunc(mousebuttonfunc);
	glutMotionFunc(mousemotionfunc);
	GLUI_Master.set_glutKeyboardFunc(keyboardfunc);
	GLUI_Master.set_glutSpecialFunc(skeyboardfunc);
	GLUI_Master.set_glutReshapeFunc(reshape);

	//GLUI *glui = GLUI_Master.create_glui_subwindow(main_win, GLUI_SUBWINDOW_BOTTOM);
	glui = GLUI_Master.create_glui_subwindow(main_win, GLUI_SUBWINDOW_BOTTOM);
	glui->set_main_gfx_window(main_win);
	
	//initialize radio button state defaults
	whichShading=0;
	whichBruteLevel=3;
	whichColor=2; whichMethod=1; seeRoutePSO=1;
	PSOinit=21;
	GLUI_Panel *h = glui->add_panel("3D Alignment",1);
	glui->add_button_to_panel(h ,"Load",0,(GLUI_Update_CB)modelfile);
	glui->add_checkbox_to_panel(h,"see Alignment Axes",&seeAxes);
	glui->add_separator_to_panel(h);
	glui->add_statictext_to_panel(h,"Optimization Method");
	glui->add_statictext_to_panel(h,"Min Area Based");
	GLUI_RadioGroup *r = glui->add_radiogroup_to_panel(h,&whichMethod);
		glui->add_radiobutton_to_group(r, "MultiLevel");
		glui->add_radiobutton_to_group(r, "PSO");
		glui->add_radiobutton_to_group(r, "GA");
	GLUI_Spinner *w = glui->add_spinner_to_panel(h,"PSO initial particles",2,&PSOinit);
	w->set_int_limits(1,42,1);
	GLUI_Spinner *v = glui->add_spinner_to_panel(h,"PSO Global iterations",2,&PSOiteration);
	v->set_int_limits(5,20,1);
	GLUI_Spinner *inert = glui->add_spinner_to_panel(h,"PSO inertia",3,&PSOinertia);
	inert->set_float_limits(0.4,1.2,1);
	GLUI_Spinner *localW = glui->add_spinner_to_panel(h,"local weight",3,&PSOlocal);
	localW->set_float_limits(0.4,1,1);
	GLUI_Spinner *globalW = glui->add_spinner_to_panel(h,"global weight",3,&PSOglobal);
	globalW->set_float_limits(1,2,1);
	glui->add_checkbox_to_panel(h,"see PSORoute",&seeRoutePSO);
	glui->add_checkbox_to_panel(h,"see Saliency",&saliencyMode);
	glui->add_button_to_panel(h ,"Align!",0,(GLUI_Update_CB)viewPoint);
	glui->add_column_to_panel(h,1);
	glui->add_statictext_to_panel(h,"Distribution Visualization");
	glui->add_separator_to_panel(h);
	glui->add_statictext_to_panel(h,"Coloring Method");
	GLUI_RadioGroup *s = glui->add_radiogroup_to_panel(h,&whichColor);
		glui->add_radiobutton_to_group(s, "Gray");
		glui->add_radiobutton_to_group(s, "RGB");
		glui->add_radiobutton_to_group(s, "HSV");
	glui->add_statictext_to_panel(h,"bruteLevel");
	GLUI_RadioGroup *t = glui->add_radiogroup_to_panel(h,&whichBruteLevel);
		glui->add_radiobutton_to_group(t, "0 ~ 12Vertices");
		glui->add_radiobutton_to_group(t, "1 ~ 42");
		glui->add_radiobutton_to_group(t, "2 ~ 162");
		glui->add_radiobutton_to_group(t, "3 ~ 642");
		glui->add_radiobutton_to_group(t, "4 ~ 2562");
		glui->add_radiobutton_to_group(t, "5 ~ 10242");
	glui->add_statictext_to_panel(h,"Shading");
	GLUI_RadioGroup *u = glui->add_radiogroup_to_panel(h,&whichShading);
		glui->add_radiobutton_to_group(u, "smooth");
		glui->add_radiobutton_to_group(u, "flat");
	glui->add_button_to_panel(h ,"Visualize",0,(GLUI_Update_CB)visualize);
	
	//glui->add_button_to_panel(h ,"pca",0,(GLUI_Update_CB)viewPointPCA);
	glui->add_separator_to_panel(h);
	glui->add_column_to_panel(h, 1);	
	glui->add_button_to_panel(h ,"Current Viewpoint",0,(GLUI_Update_CB)CurrentViewpoint);		
	glui->add_edittext_to_panel(h,"Current view area:",GLUI_EDITTEXT_FLOAT,&flapcost);
	GLUI_Spinner *xU = glui->add_spinner_to_panel(h,"xCoord",3,&xUser);
	GLUI_Spinner *yU = glui->add_spinner_to_panel(h,"yCoord",3,&yUser);
	GLUI_Spinner *zU = glui->add_spinner_to_panel(h,"zCoord",3,&zUser);
	xU->set_float_limits(-1,1,1);
	yU->set_float_limits(-1,1,1);
	zU->set_float_limits(-1,1,1);
	glui->add_button_to_panel(h ,"Rotate to this view Point",0,(GLUI_Update_CB)rotateToHere);	
	glui->add_separator_to_panel(h);glui->add_separator_to_panel(h);
       glui->add_button_to_panel(h , "Exit", 0, exit);
	resetView();

	//Go through command-line arguments and do what they say.
	for (int i = 1; i < argc-1; i++) 
	{
		if (argv[i][0] != '-')
			break;
		for (unsigned int j = 1; j < strlen(argv[i]); j++)
			keyboardfunc(argv[i][j], 0, 0);
	}
	//create the second  window for model display
		glutInitWindowSize(GW,GH);
		glutInitWindowPosition(725,100);
		second_win=glutCreateWindow("Sketch");
		printf("second_win=%d\n",second_win);
		glViewport(0,0,GW,GH );
		//set the clearcolor and the callback
		glClearColor(0.0,0.0,0.0,1.0);
		glutDisplayFunc(disp2);
		glutReshapeFunc(reshape1);
		glutMouseFunc(mymouse);
		glutKeyboardFunc(KeyFunc);
	//create the third window for visualization of cost function distribution
		glutInitWindowPosition(725,435);
		third_win=glutCreateWindow("Distribution");
		printf("third_win=%d\n",third_win);
		glViewport(0,0,GW,GH );
		//set the clearcolor and the callback
		glClearColor(0.0,0.0,0.0,1.0);
		glutDisplayFunc(disp3);
		GLUI_Master.set_glutMouseFunc(mousebuttonfunc);
		glutMotionFunc(mousemotionfunc);
		glutReshapeFunc(reshape1);
		//glutMouseFunc(mymouse);
		glutKeyboardFunc(KeyFunc);
    glutMainLoop();	
 }
void drawModel()
{
	int i, j, id, num1, num2; 
	glEnable(GL_DEPTH_TEST);
	point center=xf*themesh->bsphere.center, dummy, dummyN;
	float r=themesh->bsphere.r;	
	if (LIGHTING)
		glEnable(GL_LIGHTING); 	
	if(saliencyMode)
		glDisable(GL_LIGHTING);
	//glShadeModel(GL_FLAT);	
	glShadeModel(GL_SMOOTH);	
	xform nxf=norm_xf(xf);
	glPolygonMode(GL_FRONT, GL_FILL);
	glPolygonMode(GL_BACK, GL_FILL);
	for (i=0;i<(int)themesh->faces.size();i++)
	{
	    point v1 =xf* themesh->vertices[themesh->faces[i][0]];
		point nn1=nxf*themesh->normals[themesh->faces[i][0]];  //norm_xf(xf)*  transinverse_xf
		point v2 = xf*themesh->vertices[themesh->faces[i][1]];
		point nn2=nxf*themesh->normals[themesh->faces[i][1]];
		point v3 = xf*themesh->vertices[themesh->faces[i][2]];
		point nn3 =nxf* themesh->normals[themesh->faces[i][2]];
			
		glPointSize(1);	
		glBegin(GL_TRIANGLE_STRIP);
			if (saliencyMode){
				//subdivide vertices index into 256*256 grid
				for (j=0;j<3;j++){
					id=themesh->faces[i][j];
					num1=id/256;
					num2=id-256*num1;
					glColor3f(num2/255.0,num1/255.0,0.0);
					dummy=xf*themesh->vertices[id];
					dummy=scale_ratio*(dummy-center)/r;
					dummyN=nxf*dummy;
					if (LIGHTING)
						glNormal3fv(dummyN);
					glVertex3fv(dummy);		
				}
			}
			else{
				glColor3f(0.0,0.0,0.0);
				
				/***************first vertex*********************/
				v1=scale_ratio*(v1-center)/r;
				if (LIGHTING)
				glNormal3f(nn1[0], nn1[1], nn1[2]);
				glVertex3fv(v1);			

				
				/***************second vertex*********************/
				v2=scale_ratio*(v2-center)/r;
				if (LIGHTING)
					glNormal3f(nn2[0], nn2[1], nn2[2]);
				glVertex3fv(v2);
				
				/***************third vertex*********************/
				v3=scale_ratio*(v3-center)/r;
				if (LIGHTING)
					glNormal3f(nn3[0], nn3[1], nn3[2]);
				glVertex3fv(v3);
			}
		glEnd();
	} 
}

void drawAxes(){
	float r=themesh->bsphere.r;	
	xform nxf=norm_xf(xf);
	glDisable(GL_LIGHTING); 	
	//using bbox center is more natural in drawing the axes
	point centerB=themesh->bbox.center(), dummy1,dummy2,dummy3, nol(0,0,0);
	point centerS=themesh->bsphere.center;
	centerB=nxf*centerB; centerS=nxf*centerS;
	centerB=scale_ratio*(centerB-centerS);
	Color ccc;
	glBegin(GL_LINES);
		ccc=ccc.red();//1st axis
		glColor3fv(ccc);
		glVertex3fv(centerB);
		dummy1=nxf*alignAxes[0];
		dummy1=scale_ratio*(dummy1-centerS);
		glVertex3fv(dummy1);

		ccc=ccc.green();//2nd axis
		glColor3fv(ccc);
		glVertex3fv(centerB);
		dummy2=nxf*alignAxes[1];
		dummy2=scale_ratio*(dummy2-centerS);
		glVertex3fv(dummy2);
		
		ccc=ccc.blue();//3rd axis
		glColor3fv(ccc);
		glVertex3fv(centerB);
		dummy3=nxf*alignAxes[2];
		dummy3=scale_ratio*(dummy3-centerS);
		glVertex3fv(dummy3);
		glEnd();
}

/*----------------------------------------------------------------------------
   Rendering routine
   --------------------------------------------------------------------------*/
void disp2() 
{  
   setEnv(); 
   //glEnable(GL_HISTOGRAM);
   glMatrixMode(GL_PROJECTION); 
   glLoadIdentity(); 
   glOrtho(-AR, AR, -1, 1, -100, 100);   
   glMatrixMode(GL_MODELVIEW); 
   glLoadIdentity(); 
   glDisable(GL_LIGHTING);
   glClearColor (1.0, 1.0, 1.0, 0.0);
   if (themesh!=NULL)
   	drawModel();
   if (seeAxes)
	   drawAxes();
   //ComputeSphericalFeature();
   glutSwapBuffers();
   glutPostRedisplay();
}
void copyTriMeshToMesh(TriMesh *a, Mesh &b){
	int i,j,k;
	Mesh::face_type f;
	Mesh::vert_type v;
	point dummy;

	b.face_.clear();
	b.vert_.clear();
	b.adjface_.clear();
	b.adjvert_.clear();
	for(i=0;i<a->vertices.size();i++){
		dummy=a->vertices[i];
		D3DXVECTOR3 po(dummy[0],dummy[1],dummy[2]);
		v.p=po;
		b.vert_.push_back(v);
	}
	for(i=0;i<a->faces.size();i++){
		for(j=0;j<3;j++)
			f.vpos[j]=themesh->faces[i].v[j];
		// calc face's norm
		D3DXVECTOR3 norm;
		{
			dummy=a->vertices[a->faces[i][0]];
			D3DXVECTOR3 p0(dummy[0],dummy[1],dummy[2]);
			dummy=a->vertices[a->faces[i][1]];
			D3DXVECTOR3 p1(dummy[0],dummy[1],dummy[2]);
			dummy=a->vertices[a->faces[i][2]];
			D3DXVECTOR3 p2(dummy[0],dummy[1],dummy[2]);
			const D3DXVECTOR3 v1 = p1 - p0;
			const D3DXVECTOR3 v2 = p2 - p1;
			D3DXVec3Cross(&norm, &v1, &v2);
			D3DXVec3Normalize(&norm, &norm);
		}
		f.norm=norm;
		b.face_.push_back(f);
	}
	i=0;
}
 //Load A Model file
int  modelfile()
{
	int i,j;
	point centerMesh;
	OPENFILENAME OpenFileName;
	char szFile[MAX_PATH];

    //Create File Browser// 
	szFile[0] = 0;
	
    HWND hwnd= GetActiveWindow();              // owner window
    printf("\nhwnd=%d\n",hwnd);
	OpenFileName.lStructSize = sizeof( OPENFILENAME );
	OpenFileName.hwndOwner = (HWND)hwnd;
	//OpenFileName.lpstrFilter =(LPCSTR)"\PLY(*.ply)\0*.ply\0All Files(.*)\0*.*\0";
	OpenFileName.lpstrFilter =(LPCSTR)"OFF(*.off)\0*.off\0All Files(.*)\0*.*\0";
	OpenFileName.lpstrCustomFilter = NULL;
	OpenFileName.nMaxCustFilter = 0;
	OpenFileName.nFilterIndex = 0;
	OpenFileName.lpstrFile = (LPSTR)szFile;
	OpenFileName.nMaxFile = sizeof( szFile );
	OpenFileName.lpstrFileTitle = NULL;
	OpenFileName.nMaxFileTitle = 0;
	OpenFileName.lpstrInitialDir = NULL;
	OpenFileName.lpstrTitle = (LPCSTR)"Open a file";
	OpenFileName.nFileOffset = 0;
	OpenFileName.nFileExtension = 0;
	OpenFileName.lpstrDefExt = NULL;
	OpenFileName.lCustData = 0;
	OpenFileName.lpfnHook = NULL;
	OpenFileName.lpTemplateName = NULL;
	OpenFileName.Flags = OFN_EXPLORER;

    //Load model 
	if (GetOpenFileName(&OpenFileName)==TRUE) 
	{
		printf("\n%s\n",szFile);	
		//delete themesh;
		need_redraw();
		themesh = TriMesh::read(szFile);
		//copyTriMeshToMesh(themesh, modelMesh);
		if (themesh!=0)
		{			
			printf("Successfully loaded the model!");		
			//themesh->need_tstrips();
			themesh->need_bsphere();
			themesh->need_bbox();
			themesh->need_normals();
			//themesh->need_curvatures();
			//themesh->need_dcurv();

			/*---precompute mesh saliency---*/		
			timestamp t1 = now();
			copyTriMeshToMesh(themesh, modelMesh);	
			modelMesh.make_adjinfo();
			modelMesh.calc_normal();
			modelMesh.make_ntb();
			modelMesh.calc_curv_dir();
			modelMesh.smooth_curv();
			modelMesh.calc_saliency();
			printf("\rMesh Saliency Computation Elapsed time: %.2f sec.\n",  (now() - t1));
			/*-------------------------*/

			cout<<"Strip Size: "<<themesh->tstrips.size()<<endl;
			cout<<"Vertices Size: "<<themesh->vertices.size()<<endl;	
			cout<<"Faces Size: "<<themesh->faces.size()<<endl;	

			resetView();
			//glutSetWindow(main_win);
			//disp1();
			glutPostRedisplay();
			return TRUE;
		}
		else 
			return FALSE;
	}
	else 
	{   
		printf("Loading file  Failed!");
		return FALSE;
	}	
}

/*---------------------------------------------------------------------------
   Mouse Press p callback routine
   --------------------------------------------------------------------------*/
void mymouse(int button, int state, int x, int y)
{
	;
}

/*---------------------------------------------------------------------------
  Resize the window
  ---------------------------------------------------------------------------*/
void reshape1(int w, int h)
{
  GW = w;
  GH = h;
  AR = (float)(w)/(float)(h);
  glViewport(0, 0, w, h);               /* Set Viewport */   
  glutPostRedisplay();
}

/*---------------------------------------------------------------------------
  Set Rendering Enviroment
   --------------------------------------------------------------------------*/
void setEnv()
{
   glEnable(GL_DEPTH_TEST); 
   // Just clean the screen
   glClear(GL_COLOR_BUFFER_BIT| GL_DEPTH_BUFFER_BIT); 
   
   // lighting
  // GLfloat light_position[]={ 1.0,1.0,1.0,0.0 };
   GLfloat light_position[]={ 0.0,0.0,1.0,0.0 };

   //GLfloat light_position[]={ 0.0,0.0,10.0,0.0 };
   GLfloat white_light[]={ 1.0,1.0,1.0,1.0 };
   GLfloat lmodel_ambient[]={ 1.0,0.1,0.1,1.0 };
   glLightModeli(GL_LIGHT_MODEL_TWO_SIDE, GL_TRUE);
   glLightfv(GL_LIGHT0, GL_POSITION, light_position);
   glLightfv(GL_LIGHT0, GL_DIFFUSE, white_light);
   glLightfv(GL_LIGHT0, GL_SPECULAR, white_light);

   glLightModelfv(GL_LIGHT_MODEL_AMBIENT, lmodel_ambient);
   glEnable(GL_LIGHTING);glEnable(GL_LIGHT0);
   

   //Setup the camera
   glMatrixMode(GL_MODELVIEW); 
   glLoadIdentity(); 
   gluLookAt(0,0,20,0,0,0,0,1,0); 
   
    // setup the perspective projectin
   if (dm == GL_SELECT)
   {
	 glMatrixMode(GL_PROJECTION); 
	 //glLoadIdentity(); 
	 glOrtho(-AR, AR, -1, 1, -100, 100);
    }
}
/*---------------------------------------------------------------------------
   Key events function 
   --------------------------------------------------------------------------*/
void KeyFunc(unsigned char key, int x, int y)
{
  char sketch=0;
  switch(key)
  {      
 case 'q':
 case 'Q':
	 exit(0);
	 break;
  }
  glutPostRedisplay();
}

void disp3() 
{  
	xform nxf=norm_xf(xf);
	glLineWidth(6.0);
	point dummy1, dummy2;
   setEnv(); 
   //glEnable(GL_HISTOGRAM);
   glMatrixMode(GL_PROJECTION); 
   glLoadIdentity(); 
   glOrtho(-AR*1.25, AR*1.25, -1.25, 1.25, -100, 100);   
   glMatrixMode(GL_MODELVIEW); 
   glLoadIdentity(); 
   glDisable(GL_LIGHTING);
   glClearColor (1.0, 1.0, 1.0, 0.0);
   if(needVis) 
   {
		switch (whichBruteLevel)
		{
		case 0: drawIcos(icoMesh0);break;
		case 1: drawIcos(icoMesh1);break;
		case 2: drawIcos(icoMesh2);break;
		case 3: drawIcos(icoMesh3);break;
		case 4: drawIcos(icoMesh4);break;
		case 5: drawIcos(icoMesh5);break;
		}
   }
   if (!noOptBest.empty()){//draw bruteForce result
	   glColor3f(1,1,0);
	   dummy1=noOptBest;
		dummy2=dummy1+(float) 0.2*dummy1;
		glBegin(GL_LINES);
			glVertex3fv(nxf*dummy1);
			glVertex3fv(nxf*dummy2);
		glEnd();
   }
   if(!flock.birds.empty()&&seeRoutePSO)//draw pso route
	   drawPSO(flock);
   //ComputeSphericalFeature();
   glutSwapBuffers();
   glutPostRedisplay();
}

void drawIcos(TriMesh *mesh)
{
	int i; 
	point center=xf*mesh->bsphere.center;
	float r=mesh->bsphere.r;

	glEnable(GL_DEPTH_TEST);
	//glEnable(GL_LIGHTING); 	
	glDisable (GL_LIGHTING);
	if(whichShading) glShadeModel(GL_FLAT);	
	else glShadeModel(GL_SMOOTH);

	xform nxf=norm_xf(xf);
	glPolygonMode(GL_FRONT, GL_FILL);
	glPolygonMode(GL_BACK, GL_FILL);
	for (i=0;i<(int)mesh->faces.size();i++)
	{
		point v1 =xf* mesh->vertices[mesh->faces[i][0]];
		point nn1=nxf*mesh->normals[mesh->faces[i][0]];  //norm_xf(xf)*  transinverse_xf
		point v2 = xf*mesh->vertices[mesh->faces[i][1]];
		point nn2=nxf*mesh->normals[mesh->faces[i][1]];
		point v3 = xf*mesh->vertices[mesh->faces[i][2]];
		point nn3 =nxf* mesh->normals[mesh->faces[i][2]];
			
		glPointSize(1);
		glBegin(GL_TRIANGLES);	
			/***************first vertex*********************/
			glColor3fv(mesh->colors[mesh->faces[i][0]]);
			v1=scale_ratio*(v1-center)/r;
			if (LIGHTING)
			glNormal3f(nn1[0], nn1[1], nn1[2]);
			glVertex3fv(v1);			
	
			/***************second vertex*********************/
			glColor3fv(mesh->colors[mesh->faces[i][1]]);
			v2=scale_ratio*(v2-center)/r;
			if (LIGHTING)
				glNormal3f(nn2[0], nn2[1], nn2[2]);
			glVertex3fv(v2);
			
			/***************third vertex*********************/
			glColor3fv(mesh->colors[mesh->faces[i][2]]);
			v3=scale_ratio*(v3-center)/r;
			if (LIGHTING)
				glNormal3f(nn3[0], nn3[1], nn3[2]);
			glVertex3fv(v3);		
		glEnd();
	} 
}

void drawPSO(particlesPSO flock){
	int i, j, k;
	point dummy1, dummy2, dummy3, dummy4, dummy5;
	float scale=1.15, useless, it4, it5;
	Color ccc;
	xform nxf=norm_xf(xf);
	for(i=0;i<(int)flock.birds.size();i++){
		for(j=0;j<(int)flock.birds[i].P0s.size();j++){
			glLineWidth(3.0);
			if(!j)
				ccc=ccc.yellow();//indicating start pos
			else 
				ccc=ccc.black();//indicating subsequent pos
			glColor3fv(ccc);
			dummy1=flock.birds[i].P0s[j];
			dummy2=scale*dummy1;
			//draw particle for first and next iteration
			glBegin(GL_LINES);
				glVertex3fv(nxf*dummy1);
				glVertex3fv(nxf*dummy2);
			glEnd();
			//draw route line for 1 particle~ just use a very simple method
			if(j!=(int)flock.birds[i].P0s.size()-1){
				glLineWidth(2.0);
				ccc=ccc.red();
				glColor3fv(ccc);
				dummy3=flock.birds[i].P0s[j+1]-dummy1;//vector from first to next iteration of particle pos
				for(k=0;k<20;k++){
					it4=k/20.0; it5=(k+1)/20.0;
					dummy4=dummy1+it4*dummy3;
					dummy5=dummy1+it5*dummy3;
					decomposePt(dummy4,useless,dummy4);
					decomposePt(dummy5,useless,dummy5);
					glBegin(GL_LINES);
						glVertex3fv(nxf*(scale*dummy4));
						glVertex3fv(nxf*(scale*dummy5));
					glEnd();
				}
			}
		}
	}
	//draw current best position
	glLineWidth(5.0);
	dummy1=flock.birds[0].gBestP0;
	if(!blink){
		glColor3f(1,1,1);
		blink=1;
	}
	else{
		glColor3f(0.5,0.5,0.5);
		blink=0;
	}
	scale+=0.2;
	dummy2=scale*dummy1;
		glBegin(GL_LINES);
			glVertex3fv(nxf*dummy1);
			glVertex3fv(nxf*dummy2);
		glEnd();
}

float calVisibleSaliency()
{
	int i, j, k, index, counter=0;
	float visibleSaliency=0.0;
	int View[4];
	glutSetWindow(second_win);
	glGetIntegerv(GL_VIEWPORT, View);
	int width = View[2], height = View[3];

	for (i = 0; i < width; i++)
		for (j = 0; j < height; j++)
			for (k = 0; k < 3; k++)
				pixelColorBuf[i][j][k] = 0;

	glReadBuffer(GL_FRONT);
	glReadPixels(View[0], View[1], width, height, GL_RGB, GL_FLOAT, pixelColorBuf);
	for (i = 0; i < width; i++)
		for (j = 0; j < height; j++)  

	if (pixelColorBuf[i][j][2] == 0.0){   //pixel-level saliency
		index = int(255 *pixelColorBuf[i][j][1]) *256+int(255*pixelColorBuf[i][j][0]);
		counter++;
		if (index>= modelMesh.vert_.size())
			;//printf("out of range!\n");
		else
			visibleSaliency+=modelMesh.vert_[index].saliency;
	}
	return visibleSaliency/counter;
	}